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ABSTRACT 

A  new  robust,  optimal,  adaptive  technique  for  compensating  rate  and  position  limits  in  the  joints 
of  a  six  degree-of-freedom  elbow  manipulator  is  presented.  In  this  new  algorithm,  the  unmet 
demand  as  a  result  of  actuator  saturation  is  redistributed  among  the  remaining  unsaturated  joints. 
The  scheme  is  used  to  compensate  for  inadequate  path  planning,  problems  such  as  joint  limiting, 
joint  freezing,  or  even  obstacle  avoidance,  where  a  desired  position  and  orientation  are  not 
attainable  due  to  an  unrealizable  joint  command.  Once  a  joint  encounters  a  limit,  supplemental 
commands  are  sent  to  other  joints  to  best  track,  according  to  a  selected  criterion,  the  desired 
trajectory. 

INTRODUCTION 

A  standard  six  degree-of-fieedom  elbow  manipulator  (figure  1)  has  six  independently  controlled 
joints.  The  position  and  orientation  of  the  end  effector,  each  of  which  is  described  in  three 
dimensions,  are  fully  determined  by  the  angles  of  the  joints.  As  long  as  the  jqjpropriate  joint 
angles  are  achievable,  the  desired  position  and  orientation  can  be  obtained.  However,  when  the 
specified  joint  trajectories  caimot  be  followed  due  to  a  command  beyond  the  range  of  the 
actuator,  positions  and  orientations  downstream  from  the  limited  joint  will  all  be  affected,  causing 
in  some  cases  extreme  deviations  firom  the  e^)ected  values.  The  Windup  Feedback  scheme  [1] 
is  an  ideal  solution  candidate  for  this  problem.  It  was  designed  to  compensate  for  actuator 
saturation  in  a  multivariable  system  by  supplementing  the  commands  to  the  remaining  actuators 
to  produce  the  desired  effect  on  the  output,  in  this  case  the  gripper  position  and  orientation.  For 
each  joint  which  saturates,  a  degree  of  freedom  is  lost,  but  the  remaining  joints  can  be  used  to 
track  the  desired  path  within  the  physical  limits  of  the  manipulator. 
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MATHEMATICAL  BACKGROUND  FOR  ROBOT  JOIOT  CALCU^TIONS 
An  overview  of  the  mathematical  descriptions  used  for  robot  joint  calculations  wUl  be  presented 
in  this  section.  For  a  more  thorough  presentation,  the  reader  is  referred  to  [2]. 

In  order  to  describe  the  position  and  orientation  of  a  robot’s  end  effector  in  space,  we  will  define 
six  Cartesian  (x,  y,  z)  coordinate  frames,  one  at  each  joint.  The  main  reference  frame  is  fixed 
such  tiiat  the  base  of  the  robot  is  at  the  origin,  as  shown  in  figure  1.  Hie  five  other  reference 
frames  are  each  attached  to  one  of  the  other  joints.  Thus  the  position  and  orientation  of  the  end 
effector  with  respect  to  any  joint  is  known.  A  transformation  from  one  reference  frame^  to 
another,  consisting  of  rotations  and  translations,  can  be  described  by  the  4x4  transformation 
matrix 
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0  0  1 


T  ='r  = 


where  i  refers  to  the  original  coordinate  frame  and  j  refers  to  the  transformed  coordinate  frame. 
The  orthonormal  n-,  o-,  and  a-vectors  describe  the  orientation  as  shown  in  figure  1  while  the  p- 
vector  provides  the  position  information.  In  a  robot  manipulator,  a  transformation  matrix 
can  be  defined  to  describe  the  rotation  and  translation  required  to  get  from  the  jth  to  Ae  j+lst 
joint  lining  the  convention  that  the  motion  of  the  jth  link  is  along  the  z-axis  of  the  Jih  joint  if  it 
is  translational,  and  around  the  z-axis  of  the  Jth  joint  if  it  is  rotational.  Multiplymg  the  matrices 
describing  sequential  joint  transformations  will  give  a  new  transformation  matrix  from  the  first 
joint  in  the  series  to  the  last.  Thus,  in  a  sk-jomted  manipulator,  %  is  the  transformation  from 
the  base  to  the  gripper  in  base  coordinates,  i.e.,  %  represents  the  position  and  orientation  of  the 
end  effector  in  base  coordinates. 


Finally,  in  order  to  see  what  effect  a  differential  change  in  any  joint  (dqj  has  on  the  grqiper 
position  and  orientation,  a  6x6  matrix  known  as  the  Jacobian  is  defined.  The  Jacobim,  /,  cm 
be  used  to  conqiute  differential  changes  in  position  {dj  and  orientation  {bj  based  on  differential 
changes  in  joint  translations  and  angles  as 
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where  the  vector  dq  coireqwnds  to  differential  joint  movements,  either  translational  or  rotational, 
and  D  represents  their  corresponding  effects  at  the  gripper.  The  Jacobian  is  the  first  derivative 
of  the  equations  of  motion  with  req)ect  to  each  joint.  A  first-order  approximation  of  the^  JacoW^ 
is  easily  obtained  from  the  transformation  matrices  from  each  joint  to  the  gripper 
using  the  equations 


J  =  n'{{,b^p)  +  d) 

=  o‘{ib^p)+d) 

=  a'(ib^p)  +  d) 
b  =  n-b 
b  =  ob 
b  =  ab 

J 

where  rf  and  6  indicate  translational  and  rotational  movement  of  the  joint,  respectively.  They 
are  defined  as  J  =  (0,0,1),  6  =  (0,0,0)  for  prismatic  joints  and  </  =  (0,0,0),  6  =  (0,0,1)  for 
rotational  joints.  Using  these  relationships,  the  Jacobian  can  be  computed  as 


(»*((6xp)+<i))o  ...  (w((6xp)+<i))j 

(0-((6x;>)+i/))j^  ...  (o-((8xp)+</))5 

(a-((6xp)  +  </))^  ...  (a-((6xp)+rf))5 
"  (»*6).  •••  («-S)5 

ip'b\  ...  (0-6)5 

ia'b\  ...  (0-6)5 

where  die  subcr^ts  from  0  through  5  use  the  values  from  the  transformation  matrices  through 
%■ 

MATHEMATICAL  DEVELOPMENT  OF  THE  WINDUP  FEEDBACK  SCHEME 
The  Windup  Feedback  scheme  is  an  algorithm  developed  to  take  advantage  of  underatilized 
actuators  to  compensate  for  saturated  actuators  such  that  the  output  of  the  system  optimally  tracks 
the  output  of  a  similar  system  widiout  actuator  limits. 

In  a  robot  manipulator,  saturation  can  occur  when  a  command  to  a  joint  is  too  large  to  be 
accommodated,  either  in  position  or  rate,  such  as  a  request  to  rotate  a  joint  to  1 10*  when  it  is 
restricted  to  lie  within  the  ±90*  range,  or  a  request  to  move  110  in  one  second  when  the  rate 
limit  is  90'  per  second.  In  a  situation  where  each  joint  angle  is  computed  and  commanded  based 
on  a  desired  position  and  orientation,  a  joint  which  cannot  track  its  command  will  prevent  the 
gripper  from  reaching  its  desired  position.  By  using  other  joints  to  conqiensate  for  the  saturated 
one,  the  desired  gripper  position  can  be  nearly  matched  and  the  robot  manipulator  might  be  able 
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to  perform  its  task  as  if  no  joint  reached  its  limit.  Figure  2  depicts  a  robotic  system  with  joint 
commands  altered  by  Windup  Feedback  gains  so  that  the  position  and  orientation  of  the  end 
effector  track  their  ideal  counterparts  even  during  position  and  rate  limits.  In  figure  2,  ^  is  the 
vector  of  ideal  joint  commands,  and  A  ^  is  the  vector  of  the  difference  between  the  desired  joint 
commands  and  the  achievable  commands.  When  at  least  one  joint  is  at  its  limit,  q*  is  die  vector 
of  optimized  supplemental  commands  to  con^iensate  the  saturated  joint  commands.  If  A ^  is 
relatively  small,  it  approximates  dq  from  (1).  Using  the  definitions  from  the  previous  section, 
we  can  derive  the  Windup  Feedback  scheme  as  applied  to  manipulator  systems. 

The  Windup  Feedback  scheme  tries  to  minimize  the  difference  between  the  desired  and 
achievable  end  effector  position  and  orientation  in  an  optimal  sense.  At  every  control  interval, 
a  command  is  given  to  each  joint  with  the  goal  of  moving  the  gripper  along  a  desired  trajectory. 
If  a  desired  command  is  not  achievable  because  it  would  force  a  joint  to  move  beyond  its  limit, 
the  Windup  Feedback  scheme  will  try  to  utilize  other,  unsaturated  joints  to  maneuver  the  effector 
to  the  desired  position  and  orientation  at  the  current  time  step.  Thus,  the  quadratic  performance 
index,  PI,  for  this  optimization  procedure  is  defined  as 


PI  =  -{[/(A?  -  rr^q')YQ{J{^q  -  rr''q’y\  +  q‘''rr^Rrr^q')  (2) 
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As  shown  in  figure  2,  Lq  is  the  vector  of  unmet  demand,  i.e.  the  difference  between  the  desired 
joint  conunands  and  the  achievable  commands  when  a  joint  is  at  its  limit.  Thus,  JLq 
{^proximates  the  differential  change  in  gripper  position  and  orientation,  D,  from  (1),  required  to 
move  to  the  desired  location  based  on  the  ideal  commands.  The  vector  q*  consists  of  the 
optimized  supplemental  commands  to  compensate  the  saturated  joint  commands  as  shown  in 
figure  2.  The  diagonal  weighting  matrix  Q  allows  more  importance  to  be  given  to  selected 
variables,  such  as  position  over  orientation.  The  diagonal  weighting  matrix  R  penalizes  the  use 
of  particular  joints  for  compensation,  and  /*  is  a  matrix  which  restricts  the  supplemental  joint 
commands  to  be  distributed  over  the  unsaturated  joints,  t  is  created  by  taking  the  identity  matrix 
of  dimension  equal  to  the  number  of  joints  and  deleting  each  column  which  corresponds  to  a 
command  greater  than  the  joint’s  limit.  This  way,  whenever  a  limit  is  encountered,  /*  is 
computed  to  be  the  dimension  of  the  total  number  of  joints  by  the  total  number  of  unlimited 
joints.  In  the  objective  function  (2)  above,  the  formulation  using  two  quadratic  terms, 
corresponding  to  Q  and  R,  provides  a  great  advantage  over  the  strict  least  squares  formulation 
{Q  only),  as  will  be  shown. 

The  Windup  Feedback  gains  are  obtained  by  minimizing  (2)  with  respect  to  q*  (see  Appendix 
A  for  the  derivation)  to  produce  the  solution 


q'  =  +  r'^Rry^r'^j^QjLq 


(3) 
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The  eleTPents  of  q  are  the  supplranaital  control  commands  which,  when  added  to  the  commands 
to  the  unlimited  joints,  bring  the  end  effector  closer  to  the  desired  position  and  orientation.  The 
unmet  demand,  A  can  be  represented  as 
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where  e,  is  a  column  vector  of  zeroes  with  a  1  in  the  fth  location.  The  breaking  rq)  of  the  vector 
of  unmet  demand  into  its  individual  components  allows  each  saturated  joint  to  compensated 
individually.  Thus,  if  a  single  joint  encounters  its  limit,  a  single  colunm  of  the  Windi^  Fe^back 
matrix  can  be  computed  using  (3)  with  an  t  matrix  equal  to  the  identity  matrix  with  Ae 
appropriate  column  deleted.  If,  after  the  addition  of  the  supplemental  q*  terms,  another  joint 
saturates,  the  overdemand  is  again  redistributed  among  the  remaining  unsatur^ed  actuators 
through  a  second  column  of  the  Windup  Feedback  matrix  determined  using  a  new  T  equal  to  ^ 
previous  f  with  a  second  column  deleted.  This  process  can  continue  as  long  as  at  least  one  joint 
is  not  fuUy  utilized.  Thus,  the  ability  to  break  up  the  Lq  vector  into  its  con^nents  permits 
individual  columns  of  the  feedback  matrix  to  be  computed  as  needed.  Using  this  technique,  the 
computed  columns  correqjond  only  to  the  saturated  joints  and  allow  redistribution  only  to  the 
joints,  while  the  gains  are  continuously,  optimally  updated.  This  promotes  the 
smooth  flow  of  compensation  between  joint  commands  because,  immediately  after  a  joint 
saturates,  the  overdemand  to  it  is  small  so,  as  it  grows,  the  supplemental  commands  fed  to  the 
unsaturated  joints  are  smooth,  continuous  signals. 

As  stated  earlier,  the  inclusion  of  the  weighting  matrix  in  the  objective  fun«ion  benefits  the 
solution  greatly.  Even  though  the  addition  of  the  R  term  means  that  the  solution  obtained  will 
not  be  strictly  the  best  achievable  match  in  a  least  squares  sense  to  the  desired  solution,  it  forces 
the  siqpplemental  commands  to  stay  close  to  flieir  nominal  values  and  thereby  limits  severe  jimps 
and  sign  changes  in  the  computed  gains,  effectively  acting  as  a  smoothing  filter  for  the  time- 
varying  gains  and  resulting  in  a  potentially  much  less  erratic  set  of  supplemental  commands. 
Perhaps  more  importantly  from  an  implementation  standpoint,  the  inclusion  of  R  guarantees  Ae 
invertibility  of  the  matrix  in  (3).  Using  only  the  weighting  matrix  Q  (/?=0x/g),  the  invertibility 
of  the  matrix  is  not  guaranteed  as  the  manipulator  moves  through  its  workspace,  even  Q  is 
invertible.  When  joints  are  lined  up  along  an  axis,  such  as  when  the  robot  arm  is  straight,  Ae 
Jacobian,  /,  may  become  rank-deficient  or  at  least  have  an  unreliable  numerical  inverse.  Using 
the  above  formulation,  with  the  inclusion  of  the  matrix  R,  the  matrix  to  invert  is  in  the  Modified 
form  [3],  and  in  this  special  case  it  is  nonsingular  since  R  is  invertible;  it  does  not  depend  upon 
the  rank  of  /.  See  Appendix  B  for  a  derivation  of  this  result. 

EXAMPLES 

A  six-jointed  elbow  manipulator,  such  as  that  shown  in  figure  1,  is  used  in  two  examples  to 
demonstrate  the  Windup  Feedback  Algorithm.  The  first  illustrates  rate  limit  compeiisation,  the 
second  features  position  limit  compensation.  All  joints’  position  and  rate  liimts  are  displayed  in 
Table  I. 
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Table  1.  ELBOW  MANIPULATOR  MOTION  LIMITS 


JOINT 

POSITION  RANGE 

RATE  LIMIT 

01 

-90’  through  90’ 

90’/second 

02 

0’  through  180’ 

90’/second 

03 

-90’  through  90’ 

90’/second 

04 

-90’  through  90’ 

90’/second 

05 

0’  through  180’ 

90’/second 

06 

-90’ through  90’ 

90’/second 

The  path  planning  algorithm  used  here  simply  interpolates  from  starting  point  to  ending  point  by 
incrementing  each  joint’s  command  by  an  amount  related  to  the  distance  from  the  nearest 
en<]^int.  This  gives  a  bell-shaped  velocity  profile  (stopped  at  the  beginning,  fastest  in  the 
middle,  stopped  at  the  end).  From  figure  2  it  is  clear  that  the  Windup  Feedback  algorithm  is 
s^lied  to  the  joint  commands  only,  not  to  the  actual,  measured  joint  angles,  '^e  purpose  of  this 
scheme  is  to  provide  admissible  joint  commands,  i.e.  commands  which  the  joints  can  physically 
follow  which  will  result  in  the  desired  position  and  orientation.  Therefore,  the  way  the  limit 
checking  is  incorporated  is  significant  because  that  determines  whether  the  joints  will  truly  be 
able  to  track  the  commands.  For  these  examples,  the  rate  limit  checking  was  implemented  by 
determining  the  maximum  angle  the  joint  can  rotate  through  in  one  time  step  based  on  the 
maximum  angular  velocity  listed  in  Table  I,  not  taking  acceleration  into  accouiit,  and  allowing 
a  command  change  of  not  more  than  that  amount.  The  use  of  a  more  sophisticated  rate  limit 
checking  computation  utilizing  acceleration  limits  and  cvurent  velocity  would  not  change  Ae 
Windup  Feedback  algorithm  in  any  way.  The  weighting  matrix  Q  should  he  chosen  depending 
on  the  lask,  but  usually  the  position  is  compensated  at  the  expense  of  the  orientation,  since  most 
tasks  will  allow  a  larger  error  in  approach  than  in  position.  The  weighting  matrix  R  shoidd  be 
chosen  such  that  it  is  a  diagonal  matrix  with  aU  elements  positive.  Beyond  that,  the  Windup 
Feedback  gains  are  relatively  insensitive  to  large  changes  in  /?  as  long  as  it  is  of  the  form  R=hdg 
with  ifc>0.  In  cases  where  all  diagonal  elements  of  R  are  not  the  same,  the  potential  exists  to 
significantly  alter  the  results  by  heavily  penalizing  the  use  of  effective  joints  over  ineffective  ones 
for  compensation.  Unwise  choices  of  R  aside,  its  inclusion  should  have  very  Utde  effect  on  the 
compensated  position  and  orientation.  The  weighting  matrices  used  in  the  following  examples 
are  2=diag(100,100,100,l,l,l)  and  /?=10^5.  The  total  movement  in  each  example  takes  one 
second  witfi  the  conamands  updated  at  a  frequency  of  50  Hz. 

In  the  first  example,  the  objective  is  to  move  from  the  initial  position  and  orientation  to  the  final 
position  and  orientation  which  are  specified  as 
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Table  n  contains  the  joint  angles  corresponding  to  those  en<^oints. 

Table  H.  JOINT  ANGLES  FOR  ENDPOINTS  IN  RATE  LIMIT  EXAMPLE 


JOINT 

INITIAL  ANGLE 

FINAL  ANGLE 

01 

0” 

0' 

02 

0 

0 

0' 

03 

70* 

10' 

04 

30‘ 

60' 

05 

90' 

90' 

06 

0' 

0' 

In  the  first  example,  a  rate  limit  is  encountered  by  the  third  joint  about  one  third  of  the  way 
dirough  the  run.  The  unmet  demand  is  redirected  to  other,  unsaturated  joints  through  tihe  Windiqp 
Feedback  gains  computed  using  (3)  and  an  f  matrix  created  by  removing  the  third  column  from 
a  6x6  identity  matrix.  After  several  control  intervals,  the  supplemental  command  added  to  the 
aliea<fy  rt^idly  changing  command  to  the  second  joint  causes  it  to  rate  limit  also.  Thus,  a  second 
column  of  the  Windup  Feedback  matrix  is  computed  using  (3)  but  a  new  /*:  a  6x6  identity  matrix 
with  both  the  second  and  third  columns  removed.  The  second  joint  comes  off  its  limit  about  two 
thirds  of  the  way  through  the  run,  as  the  rate  of  change  of  the  commands  decreases,  leaving  only 
die  third  joint  saturated.  This  joint  also  comes  off  its  limit  near  the  end  of  the  run,  again  aligning 
the  compensated  and  ideal  trajectory  commands.  Figure  3  shows  in  three  dimensions  the  paths 
of  the  three  cases;  desired,  limited  without  compensation,  and  limited  with  Windup  Feedback. 
The  projections  show  that  the  error  is  limited  to  the  x-z-plane.  This  view  depicts  the  trajectories 
through  space  without  any  reference  to  time.  Thus  a  different  example  could  have  been 
concocted  where  the  saturated  curve  is  perfectly  overlaid  on  the  ideal  curve.  For  this  reason, 
figure  4  displays  the  three  curves  with  respect  to  time,  clearly  demonstrating  how  the  saturated 
case  lags  behind  the  other  two  as  the  rate-limited  joint  is  unable  to  track  the  demand.  Figure  5 
depicts  the  supplemental  command  vector,  q*,  used  to  compensate  the  saturated  command. 
Figure  6  contains  plots  of  the  joint  commands  for  the  three  cases.  In  the  63  trace,  the  rate-limited 
command  cannot  track  the  ideal  command,  resulting  in  the  immediate  divergence  of  the  other, 
compensated  joint  commands  from  the  ideal  case  to  maintain  the  end  effector  in  its  desired 
trajectory.  The  other  uncompensated  commands  track  the  ideal  commands  exactly.  The 
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condensation  is  accomplished  essentially  with  the  second  and  forth  joints,  but  when  the  second 
joint  command  also  hits  its  rate  limit,  the  other  joints  temporarily  play  a  more  prominent  r^. 
Figure  7  compares  the  error  in  gripper  position  of  the  saturated  and  compensated  cases.  The 
compensated  case  is  significantly  better  than  the  saturated  case  which  is  not  surprising  since  the 
supplemental  commands  were  optimized  to  maintain  position.  Figure  8  compos  the  enror  in 
sroproach  (the  direction  of  the  vector  a  from  figure  1,  corteqwntl^S  direction  in  which  the 
gripper  is  pointing)  for  the  two  cases.  Since  orientation  was  not  heavily  wei^ted  in  this 
example,  the  fact  that  the  compensated  case  is  much  better  is  not  significant,  but  it  shows  that 
orientation  is  not  markedly  sacrificed  to  maintain  position. 

In  the  second  example,  the  objective  is  to  move  from  the  initial  position  and  orientation  to  the 
final  position  and  orientation  which  are  specified  as 


MtiaJ 


.61  .50 

.61 

28.07 

.30 

.91 

.30 

17.32 

.71  0 

-.71 

-4.24 

T 

.71 

0 

-.71  -4.24 

00 

-.35 

0.95 

final 

-.64 

.42 

-.64  -8.90 

[  0  0 

0 

1 

0 

0 

0 

1  J 

Table  HI  contains  die  joint  angles  corresponding  to  those  endpoints. 


Table  HI.  JOINT  ANGLES  FOR  ENDPOINTS  IN  POSITION  LIMIT  EXAMPLE 


JOINT 

INITIAL  ANGLE 

FINAL  ANGLE 

0, 

0* 

0' 

02 

45' 

45' 

03 

-75' 

e 

o 

1 

04 

0' 

0' 

05  ^ 

45' 

45' 

0a 

0'  ^ 

0' 

Since  the  final  desired  value  for  the  third  joint  is  unrealizable,  every  succeeding  joint,  even  if  it 
has  achieved  its  commanded  angle,  will  not  be  at  its  desired  position  and  orientation.  In  this 
example,  the  third  joint  encounters  its  position  limit  at  nearly  half  way  through  its  desired  swing. 
The  unachievable  command  is  redistributed  through  the  Windup  Feedback  gains  to  other  joints. 
In  doing  so,  a  large  enough  supplement  is  added  to  the  fourth  joint  that  it  rides  its  rate  limit  for 
several  control  intervals  while  the  ideal  command  is  changing  at  its  fastest  rate.  This  causes  a 
second  column  of  the  Windup  Feedback  matrix  to  be  computed,  redistributing  this  unmet 
command  among  the  other  four  joints.  Once  the  rate  of  command  change  has  decreased  enough. 
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the  fourth  joint  comes  off  its  limit  while  continuing  to  acconunodate  die  unmet  command  to  the 
diird  joint.  Figure  9  shows  the  three-dimensional  path  the  end  effector  follows.  The 
compensated  path  lies  nearly  along  the  desired  trajectory  while  the  uncon^nsated  path  comes 
to  a  dead  stop  after  saturation  and  never  gets  near  its  final  destination.  Figure  10  shows  the  x-, 
y-,  and  z-positions  of  the  gripper  versus  time  for  the  three  paths.  The  compensated  path  tracks 
the  desired  closely  in  both  x  and  z  while  paying  a  small  penalty  in  y  as  compared  to  the 
uncompensated  path  which  diverges  from  the  other  two  in  both  x  and  z  after  saturation.  Figure 

11  depicts  the  supplemental  commands  used  to  compensate  die  saturated  joint  conunands.  Figure 

12  displays  ideal,  compensated,  and  saturated  commands  with  respect  to  time.  The 
uncompensated  curves  exacdy  follow  the  ideal  commands,  except  for  the  saturated  63  curve, 
which  is  the  only  one  that  shows  on  the  trace.  The  compensated  64  command’s  constant,  steep 
slope  reveals  tiiat  it  is  rate  limited  for  a  short  time  initially.  Figure  13  compares  die  error  in 
gripper  position  of  the  compensated  and  saturated  cases.  A  great  improvement  is  achieved 
through  the  use  of  Windup  Feedback  as  the  error  is  reduced  to  about  5%  of  that  in  the 
uncompensated  case  even  though  a  position  limit  was  encountered.  Figure  14  compares  the  error 
in  a^^roach  of  the  two  cases.  Here  again,  orientation  in  the  con^nsated  case  is  not  significandy 
sacrificed  to  maintain  position  and  is,  in  fact,  better  than  in  the  uncompensated  case. 

CONCLUSIONS 

The  Windiq)  Feedback  scheme  is  a  robust,  optimal  ad^tive  algorithm  which  has  been  shown  to 
significandy  improve  the  tracking  of  die  desired  end  effector  trajectory  for  a  six-degree-of- 
fteedom  elbow  manipulator  under  unexpected  rate  and  position  constraints.  The  scheme  is 
especially  suitable  for  applicadons  which  include  some  variability  so  diat  unusual  situations,  such 
as  joint  saturations,  are  likely  to  occur.  The  weighting  matrix  Q  should  be  chosen  depending 
upon  the  task,  to  appropriately  enqihasize  position  or  orientation.  The  inclusion  of  the  weighting 
matrix  R  gives  a  solution  which  is  not  the  best  fit,  in  a  least  squares  sense,  to  the  desired. 
However,  the  resulting  difference  in  position  and  orientation  between  the  optimal  solution 
obtained  using  R  and  the  least  squares  solution  should  be  negligible  and  the  compensation 
variables  should  vary  more  smoothly  than  when  R  is  not  included.  The  Windup  Feedback  gains 
are  simple  to  compute  and  adapt  online  in  real  time  which  makes  this  scheme  practical. 

APPENDIX  A:  DERIVATION  OF  WINDUP  FEEDBACK  GAINS 
The  objective  function  is  defined  as 


PI  =  -  rr^qiYQ[Ji^q  -  rr^q')]  +  q'^rr^Rrr^q') 

2 

with  variables  as  show  in  figure  2.  /*  is  created  by  taking  the  identity  matrix  of  dimension  equal 
to  the  number  of  joints  and  deleting  each  column  which  corresponds  to  a  command  greater  than 
the  joint’s  limit.  This  way,  whenever  a  limit  is  encountered,  /*  is  computed  to  be  the  dimension 
of  the  total  number  of  joints  by  the  total  number  of  unlimited  joints.  Therefore,  /*  has  more  rows 
than  columns  and  each  column  has  exactly  one  1  in  it.  It  is  clear  that  1*1*^  is  a  diagonal  matrix 
of  zeroes  and  ones  and  1*^1*  is  the  identity  matrix. 
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Example: 


/* 


1  0 

1  0 

0  0 

0  1 

,  rr^  = 

0  0 

0  1 

1  0  0 

0  0  1 

1 

0 

0 

1 

0 

1 

0 

0 

0 

0 

0 

• 

* 

II 

0 

0 

1 

0 

0 

0 

0 

1 

0 

1 

1  0 
0  1 


PI  is  easily  minimized  as  follows. 


—  =  -(jLq  -  jrr'^qyQjrr^  +  q'^rr^Rrr^  =  o 

dq’ 

-»  -rv^j^QiJLq  -  jrr^’)  +  rr^Rrr^q*  =  o 


Therefore, 


rr^J^QJLq 


r^VI'^J^QJLq 

r^J^QJt^q 


irr^j^Qjrr'^ 

(I'^rr^j^Qjrr^ 

iV^j^Qjr 


rr^Rrr^)q 

r^rr^Rrr^)q 

r^Rrr^)q 

r^Rr)r^q 


As  long  as  I'^J^QJI*  +  I*^RI‘  is  full  rank,  it  can  be  inverted,  thus 


(j'^j^Qjr  +  r^Rry^r^j^QJLq  = 

which,  using  the  identity  property  of  /*,  leads  to 


q'  =  I'ir^j^Qjr  +  r^Rry^r^j^QJ6.q 

APPENDIX  B:  PROOF  OF  INVERTIBILITY 

By  definition,  a  matrix  A  is  said  to  be  positive  semidefinite  (p.s.d.)  if  and  only  if  x^Ax>0  for  any 
vector  X.  In  the  case  where  equality  holds  only  when  x  is  uniquely  the  zero  vector,  A  is  said  to 
be  positive  definite  (p.d.)  [4].  The  eigenvalues  of  a  positive  semidefinite  matrix  are  all 
nonnegative.  The  eigenvalues  of  a  positive  definite  matrix  are  all  positive.  Consequently,  p.d. 
matrices  are  also  p.s.d.  but  they  are  always  invertible  since  all  of  their  eigenvalues  are  nonzero. 

The  inclusion  of  the  diagonal  weighting  matrix  R  in  the  objective  function  (2)  changes  the  matrix 
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to  be  inverted  in  (3)  from  being  positive  semidefinite  to  being  positive  definite  and  thus  always 
invertible.  This  is  easily  shown  as  follows. 

The  matrix  to  be  inverted  is: 


r^j^Qjr  +  r^Rr  (4) 

The  weighting  matrix  Q  is  diagonal  positive  semidefinite  (it  may  have  some  diagonal  terms  equal 
to  zero)  and  R  is  diagonal  positive  definite.  Note  that  a  matrix  A  is  p.s.d.  if  there  exists  a  matrix 
T  such  that  A=T^T  [5].  Clearly  both  terms  of  (4)  meet  this  condition,  therefore  they  are  both 
p.s.d.  Additionally,  the  second  term  is  p.d.  because,  independent  of  the  number  of  columns  of 
/*,  it  is  a  diagonal  matrix  with  all  elements  greater  than  zero  since  they  are  merely  selected 
diagonal  elements  of  the  original  R  matrix.  Pre-  and  postmultiplying  (4)  by  an  arbitrary  nonzero 
vector  X  gives 


x^ii'^j^Qjr  +  r^Rr)x  =  x^r^j^Qjrx  +  x^r^RVx 

which,  by  the  definition  of  a  p.s.d.  matrix,  produces  a  scalar  greater  than  or  equal  to  zero  for  the 
first  term  plus  a  scalar  greater  than  zero  for  die  second  term.  Thus,  the  sum  is  greater  than  zero 
for  any  nonzero  vector  x.  Therefore,  the  matrix  is  p.d.  and  consequently  always  invertible. 
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Figure  2.  Flobot  joint  commands  with  Windup  Feedback  compensation. 
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Figure  3.  Three-dimensional  view  with  projections,  for  the  rate-limited  example. 
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Figure  6.  Ideal,  compensated,  and  saturated  commands  in  degrees  vs.  sample  number  for  rate-limited  example. 
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Figure  7.  Error  in  gripper  position  vs.  sample  number  for  rate-limited  example. 
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Figure  8.  Angle  error  in  approach  vs.  sample  number  for  rate-limited  example. 


N 


lO  X 


19 


Figure  9.  Three-dimensional  view  with  projections,  for  the  position-limited  example. 
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Figure  11.  Supplemental  commands  in  degrees  vs.  sample  number  for  position-limited  example. 
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Figure  14.  Angle  error  In  approach  vs.  sample  number  for  position-limited  example. 
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